﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_SlamPossitioning.cs
 * \brief	This file contains the SlamPossitioning functions function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2010-02-16
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                       2010-02-16 \n
 *      First implementation of the M44_SlamErrors class. Copied from the SlamPossitioning class.
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_SlamErrors
     * 
     *  \brief  This is the class of the M44_SlamErrors function
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_SlamErrors
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_SlamStructures.cl_SlamErrorsResults SlamErrors(int[] ti_RobotCell,
                                                        M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
                                                        float[] tf_RoundingError,
                                                        float max_error_position,
                                                        cl_Map2D LowResMapGlobal,
                                                        cl_Map2D HighResMap)
         * 
         *  \brief This function computes the errors in a certain area arround the robot
         *         for the gradient and height layer as well as the global error 
         *         and their correspondent positions
         *  
         *  \param[in] int[] ti_RobotCell,
         *  \param[in] M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam
         *  \param[in] float[] tf_RoundingError,
         *  \param[in] float max_error_position,
         *  \param[in] cl_Map2D LowResMapGlobal
         *  \param[in] cl_Map2D HighResMap
         *  
         *  \param[out] M44_auxSlamStr.cl_SlamErrorsResults SlamErrorsResults
         */
        /* ------------------------------------------------------------------------- */

        public static M44_auxSlamStr.cl_SlamErrorsResults SlamErrors(int[] ti_RobotCell,
                                                        M44_auxSlamStr.cl_auxMapCell aux_MapCell_local_Slam,
                                                        float[] tf_RoundingError,
                                                        float max_error_position,
                                                        cl_Map2D LowResMapGlobal,
                                                        cl_Map2D HighResMap)
        {
            int i_i0, i_i1, i_j0, i_j1;
            int[] ti_RobotCell_tested = new int[2];
            float[] tf_XYPos = new float[2];
            bool b_cond1, b_cond2;
            float f_ex, f_ey, f_eh, f_eg, f_eh_sum, f_eg_sum;
            float f_eMaxGrad, f_eMaxHeight;
            float[,] tf_eGX = new float[HighResMap.us_Nrows, HighResMap.us_Ncols];
            float[,] tf_eGY = new float[HighResMap.us_Nrows, HighResMap.us_Ncols];
            float[,] tf_eH = new float[HighResMap.us_Nrows, HighResMap.us_Ncols];
            float f_minH, f_minG, f_minT; //minimum error in height, grad and total
            float[,] tf_LocalMap_GX, tf_LocalMap_GY, tf_LocalMap_H;
            M44_auxSlamStr.cl_SlamTest SlamTest = new M44_auxSlamStr.cl_SlamTest(2);
            M44_auxSlamStr.cl_SlamTestErrors SlamTestErrors = new M44_auxSlamStr.cl_SlamTestErrors(LowResMapGlobal);
            M44_auxSlamStr.cl_SlamErrorsResults SlamErrorsResults = new M44_auxSlamStr.cl_SlamErrorsResults(LowResMapGlobal);




            //Define the area where the robot is supposed to look
            i_i0 = (int)(max_error_position / LowResMapGlobal.us_Scale);
            i_i0 = ti_RobotCell[0] - i_i0;
            if (i_i0 < 0)
                i_i0 = 0;
            i_i1 = (int)(max_error_position / LowResMapGlobal.us_Scale);
            i_i1 = ti_RobotCell[0] + i_i1;
            if (i_i1 >= LowResMapGlobal.us_Nrows)
                i_i1 = LowResMapGlobal.us_Nrows - 1;
            i_j0 = (int)(max_error_position / LowResMapGlobal.us_Scale);
            i_j0 = ti_RobotCell[1] - i_j0;
            if (i_j0 < 0)
                i_j0 = 0;
            i_j1 = (int)(max_error_position / LowResMapGlobal.us_Scale);
            i_j1 = ti_RobotCell[1] + i_j1;
            if (i_j1 >= LowResMapGlobal.us_Ncols)
                i_j1 = LowResMapGlobal.us_Ncols - 1;

            //error calculus for each position
            f_eg_sum = 0;
            f_eh_sum = 0;
            for (int row = i_i0; row < i_i1; row++)
            {
                for (int column = i_j0; column < i_j1; column++)
                {
                    ti_RobotCell_tested[0] = row;
                    ti_RobotCell_tested[1] = column;

                    //Capture Maps to compare
                    tf_LocalMap_GX = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(1, LowResMapGlobal, HighResMap, ti_RobotCell_tested);
                    tf_LocalMap_GY = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(2, LowResMapGlobal, HighResMap, ti_RobotCell_tested);
                    tf_LocalMap_H = M44_auxSlamFunctions.AdaptLRM2LocalMap_Slam(3, LowResMapGlobal, HighResMap, ti_RobotCell_tested);
                    f_eMaxGrad = 0;
                    f_eMaxHeight = 0;
                    for (int i_index = 0; i_index < HighResMap.us_Nrows; i_index++)
                    {
                        for (int j_index = 0; j_index < HighResMap.us_Ncols; j_index++)
                        {
                            if (Math.Abs(aux_MapCell_local_Slam.Measured_Heights[i_index, j_index]) < 0.01)
                            {
                                tf_LocalMap_H[i_index, j_index] = 0;
                                aux_MapCell_local_Slam.Measured_Heights[i_index, j_index] = 0;
                            }
                            else
                                b_cond1 = true;
                            b_cond1 = (Math.Abs(aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index]) < 0.0001);
                            b_cond2 = (Math.Abs(aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index]) < 0.0001);
                            if (b_cond1 && b_cond2)
                            {
                                tf_LocalMap_GX[i_index, j_index] = 0;
                                tf_LocalMap_GY[i_index, j_index] = 0;
                                aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index] = 0;
                                aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index] = 0;
                            }
                            //Maximum Error calculation based on gradient and height layer
                            tf_eGX[i_index, j_index] = Math.Abs(tf_LocalMap_GX[i_index, j_index] - aux_MapCell_local_Slam.Measured_Gradient_X[i_index, j_index]);
                            tf_eGY[i_index, j_index] = Math.Abs(tf_LocalMap_GY[i_index, j_index] - aux_MapCell_local_Slam.Measured_Gradient_Y[i_index, j_index]);
                            tf_eH[i_index, j_index] = Math.Abs(tf_LocalMap_H[i_index, j_index] - aux_MapCell_local_Slam.Measured_Heights[i_index, j_index]);
                            f_eMaxGrad = Math.Max(tf_eGX[i_index, j_index] + tf_eGY[i_index, j_index], f_eMaxGrad);
                            f_eMaxHeight = Math.Max(tf_eH[i_index, j_index], f_eMaxHeight);
                        }
                    }
                    //Now we compute the errors for each parameter (height, GX and GY)
                    f_ex = M44_auxSlamFunctions.error_tests(tf_LocalMap_GX, aux_MapCell_local_Slam.Measured_Gradient_X, HighResMap.us_Nrows, HighResMap.us_Ncols);
                    f_ey = M44_auxSlamFunctions.error_tests(tf_LocalMap_GY, aux_MapCell_local_Slam.Measured_Gradient_Y, HighResMap.us_Nrows, HighResMap.us_Ncols);
                    f_eg = (float)(Math.Pow((double)f_ex, 2) + Math.Pow((double)f_ey, 2));
                    SlamTestErrors.Gradient[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eg;

                    //Minimum square test error based on heith layer
                    f_eh = M44_auxSlamFunctions.error_tests(tf_LocalMap_H, aux_MapCell_local_Slam.Measured_Heights, HighResMap.us_Nrows, HighResMap.us_Ncols);
                    SlamTestErrors.Height[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eh;

                    //Maximum error based on gradient layer
                    SlamTestErrors.Gradient_max_abs[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eMaxGrad;

                    //Maximum error based on height layer
                    SlamTestErrors.Height_max_abs[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eMaxHeight;

                    //Total error calculum
                    SlamTestErrors.Gradient_total_error[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eg * f_eMaxGrad;
                    SlamTestErrors.Height_total_error[ti_RobotCell_tested[0], ti_RobotCell_tested[1]] = f_eh * f_eMaxHeight;

                    //Acummulated error
                    f_eh_sum = f_eh_sum + (f_eh * f_eMaxHeight);
                    f_eg_sum = f_eg_sum + (f_eg * f_eMaxGrad);
                }
            }
            SlamTestErrors.Sum_errors.Err_sum_grad = f_eg_sum / ((i_i1 - i_i0 + 1) * (i_j1 - i_j0 + 1));
            SlamTestErrors.Sum_errors.Err_sum_height = f_eh_sum / ((i_i1 - i_i0 + 1) * (i_j1 - i_j0 + 1));
            //Calculate the global total error
            for (int row = i_i0; row < i_i1; row++)
            {
                for (int column = i_j0; column < i_j1; column++)
                {
                    f_eg = SlamTestErrors.Gradient_total_error[row, column] / SlamTestErrors.Sum_errors.Err_sum_grad;
                    f_eh = SlamTestErrors.Height_total_error[row, column] / SlamTestErrors.Sum_errors.Err_sum_height;
                    SlamTestErrors.Global_total_error[row, column] = f_eg * f_eh;
                }
            }
            //Filling the slam test results struct
            f_minG = SlamTestErrors.Gradient_total_error[i_i0, i_j0];
            f_minH = SlamTestErrors.Height_total_error[i_i0, i_j0];
            f_minT = SlamTestErrors.Global_total_error[i_i0, i_j0];
            for (int row = i_i0; row <= i_i1; row++)
            {
                for (int column = i_j0; column <= i_j1; column++)
                {
                    if (SlamTestErrors.Height_total_error[row, column] < f_minH)
                    {
                        f_minH = SlamTestErrors.Height_total_error[row, column];
                        SlamTest.Height_total_error.Error_value = f_minH;
                        SlamTest.Height_total_error.robotCellEstimate[0] = row;
                        SlamTest.Height_total_error.robotCellEstimate[1] = column;
                    }
                    if (SlamTestErrors.Gradient_total_error[row, column] < f_minG)
                    {
                        f_minH = SlamTestErrors.Gradient_total_error[row, column];
                        SlamTest.Gradient_total_error.Error_value = f_minG;
                        SlamTest.Gradient_total_error.robotCellEstimate[0] = row;
                        SlamTest.Gradient_total_error.robotCellEstimate[1] = column;
                    }
                    if (SlamTestErrors.Global_total_error[row, column] < f_minT)
                    {
                        f_minH = SlamTestErrors.Global_total_error[row, column];
                        SlamTest.Global_total_error.Error_value = f_minT;
                        SlamTest.Global_total_error.robotCellEstimate[0] = row;
                        SlamTest.Global_total_error.robotCellEstimate[1] = column;
                    }
                }
            }
            //Positions
            tf_XYPos = M44_BasicClass.Cell2Coordinates(LowResMapGlobal, SlamTest.Height_total_error.robotCellEstimate);
            SlamTest.Height_total_error.robotPositionEstimate[0] = tf_XYPos[0] + tf_RoundingError[0];
            SlamTest.Height_total_error.robotPositionEstimate[1] = tf_XYPos[1] + tf_RoundingError[2];

            tf_XYPos = M44_BasicClass.Cell2Coordinates(LowResMapGlobal, SlamTest.Gradient_total_error.robotCellEstimate);
            SlamTest.Gradient_total_error.robotPositionEstimate[0] = tf_XYPos[0] + tf_RoundingError[0];
            SlamTest.Gradient_total_error.robotPositionEstimate[1] = tf_XYPos[1] + tf_RoundingError[2];

            tf_XYPos = M44_BasicClass.Cell2Coordinates(LowResMapGlobal, SlamTest.Global_total_error.robotCellEstimate);
            SlamTest.Global_total_error.robotPositionEstimate[0] = tf_XYPos[0] + tf_RoundingError[0];
            SlamTest.Global_total_error.robotPositionEstimate[1] = tf_XYPos[1] + tf_RoundingError[2];

            SlamErrorsResults.SlamTest = SlamTest;
            SlamErrorsResults.SlamTestErrors = SlamTestErrors;

            return SlamErrorsResults;


        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */